
#pragma once
#include "rclcpp/rclcpp.hpp"
#include <serial/serial.h>
#include <stdint.h>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <memory>
#include <boost/enable_shared_from_this.hpp>
#include <boost/asio.hpp>
#include <boost/asio/deadline_timer.hpp>
#include <boost/asio/io_service.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/ip/udp.hpp>
#include <boost/asio/read_until.hpp>
#include <boost/asio/streambuf.hpp>
#include <boost/asio/write.hpp>
#include <boost/circular_buffer.hpp> 
#include <socketcan_interface/socketcan.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>

#define PACKHEAD_LENGTH     10
#define PAYLOAD_MAX_LENGTH  256
#define PACKAGE_MAX_LENGTH	PAYLOAD_MAX_LENGTH + PACKHEAD_LENGTH


#define FWCORSS          1       // Forword Zero crossing
#define RSCORSS          2       // Reverse Zero crossing
#define NOCORSS          0       // No Zero crossing

typedef struct {                 // config for ethernet 
    boost::asio::io_service *io; // I/O service for asynchronous I/O operations  
    boost::thread* service; 
    int port;                    // Ethernet communication port              
} Ethernet;

typedef struct {
	std::string *device_name;     // can_device
	int nodeId;				     // nodeId
	int protocol; 				 // protocol: canopen\normal
} Canbus;

union ConnetionParam {
    std::string *device_name;    // serial port name
    int bandrate;                // serial port bandrate
    Ethernet eth;                // ethernet config
	Canbus can;                  // can bus config
};

enum ConnectionType {
    Serial = 0,
    Tcp = 1,
    Udp = 2,
    Can = 3
};

enum EncoderDirection {
  Unknown = 0,
  Forward = 1,
  Reverse = -1
};

// Packet Structure
struct Packet {
    uint16_t header;
    uint16_t seq;
    uint16_t size;
    std::vector<uint8_t> payload;
    uint16_t crc;
    uint16_t tail;
};

namespace Sensor {
    //*********************************************//
    /// Communication Base class
    class Communication  {
    public: 
        explicit Communication(std::shared_ptr<rclcpp::Node> node):
            node_(node) {}
            
        virtual ~Communication() = default;

        virtual int init() = 0;
        virtual void write(std::string msg) = 0;
        virtual uint64_t read(const uint8_t index = 0) = 0;
        virtual void update() = 0;
        virtual int64_t get_encoder_turns(const int index = 0) = 0;

        std::shared_ptr<rclcpp::Node> node_;
    };
    
    /// Derived class for serial communication
    class SerialComm : public Communication {
    public:
        /// Constructor
        SerialComm(std::shared_ptr<rclcpp::Node> node, 
            const std::string port, 
            const int baudRate, 
            const double &rate);
        
        /// Destructor
        ~SerialComm();

        /// Initialize serial port with given parameters
        int init() override;

        /// Implementation for sending message over serial port
        void write(const std::string msg) override;
            
        /// Implementation for receiving message over serial port
        uint64_t read(const uint8_t index = 0) override;

        /// Implementation for update thread data
        void update() override;
        

        int64_t get_encoder_turns(const int index = 0) override
        {
            return 0;
        }
    protected:
        serial::Serial serial_;
        double loopHz_;
        uint64_t encoder_;
        int bandrate_;
    };

    // ******************************************************************//
    const int BUFFER_MAX_LEN = 2048;
    
    /// Derived class for TCP server communication
    class TcpServerComm : public Communication {
    public:
        /// Constructor
        TcpServerComm(std::shared_ptr<rclcpp::Node> node, 
            const int portNumber, 
            boost::asio::io_service& io_service, 
            const double &rate);
        
        /// Destructor
        ~TcpServerComm();

        /// Initialize TCP server with given port number
        int init() override;

        /// Implementation for sending message over TCP server
        void write(const std::string msg) override;
        
        /// Implementation for receiving message over TCP server
        uint64_t read(const uint8_t index = 0) override;

        /// Implementation for update thread data
        void update() override;

        /// Implementation for get encoder turns;
        int64_t get_encoder_turns(const int index) override
        {
            return encoder_[index].turns;
        }

    private:
        /// Handle incoming connections on a server socket
        void handle_accept(const boost::system::error_code &ec, std::shared_ptr<boost::asio::ip::tcp::socket> socket);
        
        /// Start listening to the interface
        int start();

        /// Stop socket
        void stop();
        
        /// Get socket status
        bool status() const
        {
            return !sockPtr_->is_open();
        }

        /// Implementation for get encoder direction;
        EncoderDirection get_encoder_direction(const int index) 
        {
            return encoder_[index].dir;
        }
         /// Handler to write message to socket
        void handler_write(std::shared_ptr<std::string> pstr, boost::system::error_code ec, size_t bytes_transferred);

        /// Handler to write message to socket
        void handler_read(std::shared_ptr<boost::asio::ip::tcp::socket> socket);

        /// Implementation for encoder direction
        void check_zero_crossing(void);

        /// crc calculation
        unsigned char crc8(unsigned char *message, unsigned char len);

        /// Packet parsing
        void ParsePacket();
    private:
        double loopHz_;
        double last_data_time;
        boost::thread* parse_;
        // boost::thread* service_run_;
        boost::asio::io_service& io_service_;
        boost::asio::ip::tcp::acceptor acceptor_;
        std::shared_ptr<boost::asio::ip::tcp::socket> sockPtr_; 
        boost::circular_buffer<uint8_t> buffer_;
        
        struct ENCODER {
            EncoderDirection dir;
            int64_t turns;
            uint64_t value;
            uint32_t last_value;
        } encoder_[2]; 

        struct IMU {
            float acc[3];
            float gyro[3];
        } imu_;

        bool stopped_;
    };

    // ******************************************************************//
    /// Derived class for can communication
    class CanComm : public Communication {
    public:
        /// Constructor
        CanComm(std::shared_ptr<rclcpp::Node> node, 
            const std::string can_device, 
            const int nodeId, 
            const int protocol);
        
        /// Destructor
        ~CanComm();

        /// Initialize serial port with given parameters
        int init() override;

        /// Implementation for sending message over serial port
        void write(const std::string msg) override;
            
        /// Implementation for receiving message over serial port
        uint64_t read(const uint8_t index = 0) override;

        /// Implementation for update thread data
        void update() override;


        int64_t get_encoder_turns(const int index = 0) override
        {
            return 0;
        }

        void msg_handle(const can::Frame &msg);

        void sdoSend(uint8_t node, uint8_t rwflag, uint8_t len, uint16_t index, uint8_t subIndex, uint32_t data);

        void nmtSend(uint8_t nodeId, uint8_t cmd);
    protected:
        std::string can_device_;
        int nodeId_;
        int protocol_;
        int64_t encoder_;
        can::DriverInterfaceSharedPtr driver_;
        can::FrameListenerConstSharedPtr frame_listener_;
    };
} // end namespace
